import pyzed.sl as sl
import math

zed = sl.Camera()
# Create configuration parameters
init_params = sl.InitParameters()
init_params.sdk_verbose = True # Enable the verbose modeinit_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE 
init_params.camera_resolution = sl.RESOLUTION.HD1080 # Use HD1080 video mode
init_params.camera_fps = 30 # Set fps at 30
# Set the depth mode to performance (fastest)
# Open the camera
err = zed.open(init_params)
if (err!=sl.ERROR_CODE.SUCCESS):
    exit(-1)

def get_depth(points):
    # Capture 50 images and depth, then stop
    i = 0
    image = sl.Mat()
    depth = sl.Mat()
    point_cloud = sl.Mat()
    # Grab an image    
    if (zed.grab() == sl.ERROR_CODE.SUCCESS) :
        # A new image is available if grab() returns SUCCESS        
        zed.retrieve_image(image, sl.VIEW.LEFT) 
        # Get the left image        
        zed.retrieve_measure(depth, sl.MEASURE.DEPTH) 
        # Retrieve depth Mat. Depth is aligned on the left image
        zed.retrieve_measure(point_cloud, sl.MEASURE.XYZRGBA)          
        # Retrieve colored point cloud. Point cloud is aligned on the left image     
        i = i + 1
        distance = []
        for x,y in points:
            point_cloud_value = point_cloud.get_value(x, y)[1]
            distance.append(math.sqrt(point_cloud_value[0]*point_cloud_value[0] + point_cloud_value[1]*point_cloud_value[1] + point_cloud_value[2]*point_cloud_value[2]))
        return distance

def get_img():
    left_img = sl.Mat()
    runtimeParas = sl.RuntimeParameters() 
    if zed.grab(runtimeParas) == sl.ERROR_CODE.SUCCESS:
        zed.retrieve_image(left_img, view=sl.VIEW.LEFT)
        return left_img.get_data()  

def __main():
    while True:
        print(get_depth([[400,400],[800,800]]))
    
if __name__ == '__main__':
    __main()